
#include <msp430g2553.h>
#include "ChaosCube.h"


void IMU_write(unsigned char wdata, unsigned char * rdata, int read_sz);
void IMU_write2(unsigned char * wdata, int write_sz, unsigned char * rdata, int read_sz);
void IMU_writereg(unsigned char reg, unsigned char data);

int detect_face(void);

signed int ACCEL_XOUT;
signed int ACCEL_YOUT;
signed int ACCEL_ZOUT;

char buf[160];

volatile int shaken = 0;


int waitRoll(void) {
	
  while (!shaken) {}
  unsigned char int_status;
  IMU_write(MPU6050_RA_INT_STATUS, &int_status,1);
  sendVisual(success);

  /* Begin timeout for settling */
  initTimeOut();

   
  int face=0;
  int xc=0,yc=0,zc=0;
  while(!face){
    unsigned char ACCEL_XOUT_H;
    unsigned char ACCEL_XOUT_L;
    unsigned char ACCEL_YOUT_H;
    unsigned char ACCEL_YOUT_L;
    unsigned char ACCEL_ZOUT_H;
    unsigned char ACCEL_ZOUT_L;
    
    IMU_write(MPU6050_RA_ACCEL_XOUT_H, &ACCEL_XOUT_H, 1);
    IMU_write(MPU6050_RA_ACCEL_XOUT_L, &ACCEL_XOUT_L, 1);
    IMU_write(MPU6050_RA_ACCEL_YOUT_H, &ACCEL_YOUT_H, 1);
    IMU_write(MPU6050_RA_ACCEL_YOUT_L, &ACCEL_YOUT_L, 1);
    IMU_write(MPU6050_RA_ACCEL_ZOUT_H, &ACCEL_ZOUT_H, 1);
    IMU_write(MPU6050_RA_ACCEL_ZOUT_L, &ACCEL_ZOUT_L, 1);
   
    ACCEL_XOUT = ((ACCEL_XOUT_H<<8)|ACCEL_XOUT_L);
    ACCEL_YOUT = ((ACCEL_YOUT_H<<8)|ACCEL_YOUT_L);
    ACCEL_ZOUT = ((ACCEL_ZOUT_H<<8)|ACCEL_ZOUT_L);
  
   unsigned int xm = abs(ACCEL_XOUT);
   unsigned int ym = abs(ACCEL_YOUT);
   unsigned int zm = abs(ACCEL_ZOUT);
   
   if (xm < 750 || xm > 3000)
   	xc = 0;
   else
    xc += ACCEL_XOUT > 0 ? 1 : -1;
   if (ym < 750 || ym > 3000)
    yc = 0;
   else
    yc += ACCEL_YOUT > 0 ? 1 : -1;
   if (zm < 750 || zm > 3000)
    zc = 0;
   else
    zc += ACCEL_ZOUT > 0 ? 1 : -1;
   if ((xc&&yc)||(xc&&zc)||(yc&&zc)){
   	xc=0;
   	yc=0;
   	zc=0;
   }
   if (abs(xc) >= 10 || abs(yc) >= 10 || abs(zc) >= 10) {
   	if (xc > 0)
   	 face = face1;
   	else if (xc < 0)
   	 face = face6;
   	else if (yc > 0)
   	 face = face2;
   	else if (yc < 0)
   	 face = face5;
   	else if (zc > 0)
   	 face = face3;
   	else if (zc < 0)
   	 face = face4;
   	else
   	 face = failure;
   	break;
   }
   int imu_int = (P2IN & BIT5) == BIT5;
  
   delay_ms(100);
  }
   return face;
}

void IMU_writereg(unsigned char reg, unsigned char data) {
  unsigned char tx[2];
  tx[0] = reg;
  tx[1] = data;
  IMU_write2(tx,2,0,0);
}

void IMU_write(unsigned char wdata, unsigned  char * rdata, int read_sz) {
  while(TI_USCI_I2C_notready());
  TI_USCI_I2C_transmitinit(MPU6050_ADDRESS_AD0_LOW,15);
  while(TI_USCI_I2C_notready());
  TI_USCI_I2C_transmit(1,&wdata);
  while(TI_USCI_I2C_notready());

  if (read_sz > 0) {
    while(TI_USCI_I2C_notready());
    TI_USCI_I2C_receiveinit(MPU6050_ADDRESS_AD0_LOW,15);
    while(TI_USCI_I2C_notready());
    TI_USCI_I2C_receive(read_sz,rdata);
    while(TI_USCI_I2C_notready());
  }
}

void IMU_write2(unsigned char * wdata, int write_sz,unsigned  char * rdata, int read_sz) {
  if (write_sz > 0) {
    while(TI_USCI_I2C_notready());
    TI_USCI_I2C_transmitinit(MPU6050_ADDRESS_AD0_LOW,15);
    while(TI_USCI_I2C_notready());
    TI_USCI_I2C_transmit(write_sz,wdata);
    while(TI_USCI_I2C_notready());
  } 
  if (read_sz > 0) {
    while(TI_USCI_I2C_notready());
    TI_USCI_I2C_receiveinit(MPU6050_ADDRESS_AD0_LOW,15);
    while(TI_USCI_I2C_notready());
    TI_USCI_I2C_receive(read_sz,rdata);
    while(TI_USCI_I2C_notready());
  }
}

void initIMU(void) {
	P2SEL &= ~BIT5;
	P2SEL2 &= ~BIT5;

	  P2DIR &= ~BIT5;
	  P2REN |= BIT5;
	  P2OUT |= BIT5;

	  P3SEL &= ~BIT5;
	  P3SEL2 &= ~BIT5;
	  P3DIR &= ~BIT5;
	  P3REN |=  BIT5;
	  P3OUT |=  BIT5;



  // run accelero/gyroscope initialization
  
  //Sets sample rate to 8000/1+7 = 1000Hz
    IMU_writereg(MPU6050_RA_SMPLRT_DIV, 0x07);
    //Disable gyro self tests, scale of 250 degrees/s
    IMU_writereg(MPU6050_RA_GYRO_CONFIG, 0x18);
    //Disable accel self tests, scale of +-8g, no DHPF
    IMU_writereg(MPU6050_RA_ACCEL_CONFIG, 0x18);
    //Sets clock source to gyro reference w/ PLL & y-axis gyro reference
    IMU_writereg(MPU6050_RA_PWR_MGMT_1, 0x02);


  IMU_writereg(MPU6050_RA_INT_PIN_CFG,0xE0);
  
  IMU_writereg(MPU6050_RA_MOT_DUR,    0x01);
  IMU_writereg(MPU6050_RA_MOT_THR,    0x30);
  IMU_writereg(MPU6050_RA_MOT_DETECT_CTRL, 0x10);
  IMU_writereg(MPU6050_RA_INT_ENABLE, 0x40);
  delay_ms(1);
  IMU_writereg(MPU6050_RA_ACCEL_CONFIG, 0x1F);
  IMU_writereg(MPU6050_RA_PWR_MGMT_2,0x40);
  IMU_writereg(MPU6050_RA_PWR_MGMT_1,0x20);
  
  
  //clear interrupt
  unsigned char int_status;
  IMU_write(MPU6050_RA_INT_STATUS, &int_status,1);

  P2IFG &= ~BIT5;
  delay_ms(1000);
  P2IE  |=  BIT5;


  return;
}

// Port 2 interrupt service routine
#pragma vector=PORT2_VECTOR
__interrupt void Port_2(void)
{
	if (!(P2IN & BIT5))
		shaken = 1;
    P2IFG &= ~BIT5; // P1.3 IFG cleared
}
